#include "MarkerBasedConverter.h"

#include "../../../RapidXML/rapidxml.hpp"
#include <fstream>

using std::cout;
using std::endl;

namespace MMM
{

MarkerBasedConverter::MarkerBasedConverter(const std::string &name)
	: Converter(name)
{

}

bool MarkerBasedConverter::setup(AbstractMotionPtr inputMotion, ModelPtr outputModel)
{
	return setup(ModelPtr(), inputMotion, outputModel);
}

bool MarkerBasedConverter::setup(ModelPtr inputModel, AbstractMotionPtr inputMotion, ModelPtr outputModel)
{
	if (!inputMotion || !outputModel)
		return false;
	inputMarkerMotion = boost::dynamic_pointer_cast<MarkerMotion>(inputMotion);
	if (!inputMarkerMotion)
	{
		MMM_ERROR << "Need a MarkerMotion as input..." << endl;
		return false;
	}
	// retrieve joint order
	std::vector<ModelNodePtr> rn = outputModel->getModels();
	std::vector<std::string> joints;
	int nrDOF = 0;
	for (auto & i : rn)
	{
		if (i->joint.jointType == eRevolute)
		{
			nrDOF++;
			joints.push_back(i->name);
        }
        if (i->joint.jointType == ePrismatic)
        {
            std::cout << "MarkerBasedConverter.cpp: Prismatic Link [" << i->name << "] has initial value ["  << i->joint.initValue << "]" << std::endl;
            // HACK - test if this works...
            /*
            nrDOF++;
            joints.push_back(rn[i]->name);
            */
        }
    }
	if (nrDOF == 0)
	{
		MMM_ERROR << "No joint definitions in outputModel..." << endl;
		return false;
	}
	setupJointOrder(joints);

	return Converter::setup(inputModel, inputMotion, outputModel);
}

std::map<std::string, std::string> MarkerBasedConverter::getMarkerMapping() const
{
	return markerMapping;
}


}
